#include "Matrix.h"

#define PI 3.141592654f

Matrix::Matrix(void)
{
    LoadIdentity();
}
Matrix::Matrix(Matrix *mt)
{
	float *temp = mt->ToArray();
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = temp[i*4+j];
		}
	}
	delete []temp;
}

Matrix::Matrix(float* arr)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = arr[i*4+j];
		}
	}
}

Matrix::~Matrix(void)
{
}
void Matrix::CreateViewMatrix(float xPos, float yPos, float zPos)
{
	Translate(xPos,yPos,zPos);
}
void Matrix::CreateFrutum(float xLeft, float xRight, float yBottom, float yTop, float zNear, float zFar)
{
	float       xDelta = xRight - xLeft;
	float       yDelta = yTop - yBottom;
	float       zDelta = zFar - zNear;

	if ( (zNear <= 0.0f) || (zFar <= 0.0f) ||
		(xDelta <= 0.0f) || (yDelta <= 0.0f) || (zDelta <= 0.0f) )
		return;

	m[0][0] = 2.0f * zNear / xDelta;
	m[0][1] = m[0][2] = m[0][3] = 0.0f;

	m[1][1] = 2.0f * zNear / yDelta;
	m[1][0] = m[1][2] = m[1][3] = 0.0f;

	m[2][0] = (xRight + xLeft) / xDelta;
	m[2][1] = (yTop + yBottom) / yDelta;
	m[2][2] = -(zNear + zFar) / zDelta;
	m[2][3] = -1.0f;

	m[3][2] = -2.0f * zNear * zFar / zDelta;
	m[3][0] = m[3][1] = m[3][3] = 0.0f;
}
void Matrix::Perspective(float fovy, float aspect, float nearZ, float farZ)
{
	float frustumW, frustumH;

	frustumH = tanf( fovy / 360.0f * PI ) * nearZ;
	frustumW = frustumH * aspect;

	CreateFrutum(-frustumW, frustumW, -frustumH, frustumH, nearZ, farZ );
}
void Matrix::LoadIdentity()
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = 0.0;
		}
	}
	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
}
void Matrix::MatrixMultiply(Matrix *matrixA, Matrix *matrixB)
{
	Matrix    *tmp = new Matrix();
	for (int i=0; i<4; i++)
	{
		tmp->m[i][0] =	(matrixA->m[i][0] * matrixB->m[0][0]) +
						(matrixA->m[i][1] * matrixB->m[1][0]) +
						(matrixA->m[i][2] * matrixB->m[2][0]) +
						(matrixA->m[i][3] * matrixB->m[3][0]) ;

		tmp->m[i][1] =	(matrixA->m[i][0] * matrixB->m[0][1]) + 
						(matrixA->m[i][1] * matrixB->m[1][1]) +
						(matrixA->m[i][2] * matrixB->m[2][1]) +
						(matrixA->m[i][3] * matrixB->m[3][1]) ;

		tmp->m[i][2] =	(matrixA->m[i][0] * matrixB->m[0][2]) + 
						(matrixA->m[i][1] * matrixB->m[1][2]) +
						(matrixA->m[i][2] * matrixB->m[2][2]) +
						(matrixA->m[i][3] * matrixB->m[3][2]) ;

		tmp->m[i][3] =	(matrixA->m[i][0] * matrixB->m[0][3]) + 
						(matrixA->m[i][1] * matrixB->m[1][3]) +
						(matrixA->m[i][2] * matrixB->m[2][3]) +
						(matrixA->m[i][3] * matrixB->m[3][3]) ;
	}
	this->Copy(tmp);
	delete tmp;

}

void Matrix::MatrixSubtraction(Matrix *matrixA, Matrix *matrixB)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = matrixA->m[i][j] - matrixB->m[i][j];
		}
	}
}
void Matrix::MatrixSum(Matrix *matrixA, Matrix *matrixB)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = matrixA->m[i][j] + matrixB->m[i][j];
		}
	}
}
void Matrix::Transpose()
{
	Matrix *temp = new Matrix(this);
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = temp->m[j][i];
		}
	}
	delete temp;
}
void Matrix::RotateX(float angle)
{
	float sinAngle, cosAngle;
	sinAngle = sinf ( angle * PI / 180.0f );
	cosAngle = cosf ( angle * PI / 180.0f );
	Matrix temp;
	temp.LoadIdentity();
	temp.m[1][1] = temp.m[2][2] = cosAngle;
	temp.m[1][2] = sinAngle;
	temp.m[2][1] = -sinAngle;
	MatrixMultiply(&temp,this);
}
void Matrix::RotateY(float angle)
{
	float sinAngle, cosAngle;
	sinAngle = sinf ( angle * PI / 180.0f );
	cosAngle = cosf ( angle * PI / 180.0f );
	Matrix temp;
	temp.LoadIdentity();
	temp.m[0][0] = temp.m[2][2] = cosAngle;
	temp.m[2][0] = sinAngle;
	temp.m[0][2] = -sinAngle;
	MatrixMultiply(&temp,this);
}
void Matrix::RotateZ(float angle)
{
	float sinAngle, cosAngle;
	sinAngle = sinf ( angle * PI / 180.0f );
	cosAngle = cosf ( angle * PI / 180.0f );
	Matrix temp;
	temp.LoadIdentity();
	temp.m[0][0] = temp.m[1][1] = cosAngle;
	temp.m[0][1] = sinAngle;
	temp.m[1][0] = -sinAngle;
	MatrixMultiply(&temp,this);
}
void Matrix::Rotate(float angle, float x, float y, float z)
{
	float sinAngle, cosAngle;
	float mag = sqrtf(x * x + y * y + z * z);

	sinAngle = sinf ( angle * PI / 180.0f );
	cosAngle = cosf ( angle * PI / 180.0f );

	if ( mag > 0.0f )
	{
		float xx, yy, zz, xy, yz, zx, xs, ys, zs;
		float oneMinusCos;
		Matrix rotMat;
		x /= mag;
		y /= mag;
		z /= mag;

		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * sinAngle;
		ys = y * sinAngle;
		zs = z * sinAngle;
		oneMinusCos = 1.0f - cosAngle;

		rotMat.m[0][0] = (oneMinusCos * xx) + cosAngle;
		rotMat.m[0][1] = (oneMinusCos * xy) - zs;
		rotMat.m[0][2] = (oneMinusCos * zx) + ys;
		rotMat.m[0][3] = 0.0F; 

		rotMat.m[1][0] = (oneMinusCos * xy) + zs;
		rotMat.m[1][1] = (oneMinusCos * yy) + cosAngle;
		rotMat.m[1][2] = (oneMinusCos * yz) - xs;
		rotMat.m[1][3] = 0.0F;

		rotMat.m[2][0] = (oneMinusCos * zx) - ys;
		rotMat.m[2][1] = (oneMinusCos * yz) + xs;
		rotMat.m[2][2] = (oneMinusCos * zz) + cosAngle;
		rotMat.m[2][3] = 0.0F; 

		rotMat.m[3][0] = 0.0F;
		rotMat.m[3][1] = 0.0F;
		rotMat.m[3][2] = 0.0F;
		rotMat.m[3][3] = 1.0F;

		MatrixMultiply( &rotMat, this);
	}
}

void Matrix::Translate(float x, float y, float z)
{
	for(int i=0;i<4;i++)
		m[3][i] += m[0][i] * x + m[1][i] * y + m[2][i] * z;
}
void Matrix::Scale(float x, float y, float z)
{
	for(int i=0;i<4;i++)
	{
		m[0][i] *= x;
		m[1][i] *= y;
		m[2][i] *= z;
	}
}
float* Matrix::ToArray()
{
	float *fArray = new float[16];
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			fArray[i*4+j] = m[i][j];
		}
	}
	return fArray;
}

void Matrix::Copy(Matrix *src)
{
	float *temp = src->ToArray();
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m[i][j] = temp[i*4+j];
		}
	}
	delete []temp;
}

Matrix* Matrix::GetInvert()
{
	float inv[16], det;
		inv[0] =   m[1][1]*m[2][2]*m[3][3] - m[1][1]*m[3][2]*m[2][3] - m[1][2]*m[2][1]*m[3][3]
		+ m[1][2]*m[3][1]*m[2][3] + m[1][3]*m[2][1]*m[3][2] - m[1][3]*m[3][1]*m[2][2];
		inv[1] =  -m[0][1]*m[2][2]*m[3][3] + m[0][1]*m[3][2]*m[2][3] + m[0][2]*m[2][1]*m[3][3]
		- m[0][2]*m[3][1]*m[2][3] - m[0][3]*m[2][1]*m[3][2] + m[0][3]*m[3][1]*m[2][2];
		inv[2] =   m[0][1]*m[1][2]*m[3][3] - m[0][1]*m[3][2]*m[1][3] - m[0][2]*m[1][1]*m[3][3]
		+ m[0][2]*m[3][1]*m[1][3] + m[0][3]*m[1][1]*m[3][2] - m[0][3]*m[3][1]*m[1][2];
		inv[3] = -m[0][1]*m[1][2]*m[2][3] + m[0][1]*m[2][2]*m[1][3] + m[0][2]*m[1][1]*m[2][3]
		- m[0][2]*m[2][1]*m[1][3] - m[0][3]*m[1][1]*m[2][2] + m[0][3]*m[2][1]*m[1][2];
		inv[4] =  -m[1][0]*m[2][2]*m[3][3] + m[1][0]*m[3][2]*m[2][3] + m[1][2]*m[2][0]*m[3][3]
		- m[1][2]*m[3][0]*m[2][3] - m[1][3]*m[2][0]*m[3][2] + m[1][3]*m[3][0]*m[2][2];
		inv[5] =   m[0][0]*m[2][2]*m[3][3] - m[0][0]*m[3][2]*m[2][3] - m[0][2]*m[2][0]*m[3][3]
		+ m[0][2]*m[3][0]*m[2][3] + m[0][3]*m[2][0]*m[3][2] - m[0][3]*m[3][0]*m[2][2];
		inv[6] =  -m[0][0]*m[1][2]*m[3][3] + m[0][0]*m[3][2]*m[1][3] + m[0][2]*m[1][0]*m[3][3]
		- m[0][2]*m[3][0]*m[1][3] - m[0][3]*m[1][0]*m[3][2] + m[0][3]*m[3][0]*m[1][2];
		inv[7] =  m[0][0]*m[1][2]*m[2][3] - m[0][0]*m[2][2]*m[1][3] - m[0][2]*m[1][0]*m[2][3]
		+ m[0][2]*m[2][0]*m[1][3] + m[0][3]*m[1][0]*m[2][2] - m[0][3]*m[2][0]*m[1][2];
		inv[8] =   m[1][0]*m[2][1]*m[3][3] - m[1][0]*m[3][1]*m[2][3] - m[1][1]*m[2][0]*m[3][3]
		+ m[1][1]*m[3][0]*m[2][3] + m[1][3]*m[2][0]*m[3][1] - m[1][3]*m[3][0]*m[2][1];
		inv[9] =  -m[0][0]*m[2][1]*m[3][3] + m[0][0]*m[3][1]*m[2][3] + m[0][1]*m[2][0]*m[3][3]
		- m[0][1]*m[3][0]*m[2][3] - m[0][3]*m[2][0]*m[3][1] + m[0][3]*m[3][0]*m[2][1];
		inv[10] =  m[0][0]*m[1][1]*m[3][3] - m[0][0]*m[3][1]*m[1][3] - m[0][1]*m[1][0]*m[3][3]
		+ m[0][1]*m[3][0]*m[1][3] + m[0][3]*m[1][0]*m[3][1] - m[0][3]*m[3][0]*m[1][1];
		inv[11] = -m[0][0]*m[1][1]*m[2][3] + m[0][0]*m[2][1]*m[1][3] + m[0][1]*m[1][0]*m[2][3]
		- m[0][1]*m[2][0]*m[1][3] - m[0][3]*m[1][0]*m[2][1] + m[0][3]*m[2][0]*m[1][1];
		inv[12] =  -m[1][0]*m[2][1]*m[3][2] + m[1][0]*m[3][1]*m[2][2] + m[1][1]*m[2][0]*m[3][2]
		- m[1][1]*m[3][0]*m[2][2] - m[1][2]*m[2][0]*m[3][1] + m[1][2]*m[3][0]*m[2][1];
		inv[13] =   m[0][0]*m[2][1]*m[3][2] - m[0][0]*m[3][1]*m[2][2] - m[0][1]*m[2][0]*m[3][2]
		+ m[0][1]*m[3][0]*m[2][2] + m[0][2]*m[2][0]*m[3][1] - m[0][2]*m[3][0]*m[2][1];
		inv[14] = -m[0][0]*m[1][1]*m[3][2] + m[0][0]*m[3][1]*m[1][2] + m[0][1]*m[1][0]*m[3][2]
		- m[0][1]*m[3][0]*m[1][2] - m[0][2]*m[1][0]*m[3][1] + m[0][2]*m[3][0]*m[1][1];
		inv[15] =  m[0][0]*m[1][1]*m[2][2] - m[0][0]*m[2][1]*m[1][2] - m[0][1]*m[1][0]*m[2][2]
		+ m[0][1]*m[2][0]*m[1][2] + m[0][2]*m[1][0]*m[2][1] - m[0][2]*m[2][0]*m[1][1];

		det = m[0][0]*inv[0] + m[1][0]*inv[1] + m[2][0]*inv[2] + m[3][0]*inv[3];
		if (det == 0)
				return 0;

		det = 1.0 / det;
		for (int i = 0; i < 16; i++)
				inv[i] = inv[i] * det;
		Matrix *result = new Matrix(inv);
		return result;
}

float Matrix::GetValue(int row, int col)
{
	return m[row][col];
}

void Matrix::Ortho(float left, float right, float top, float bottom, float near, float far){
    LoadIdentity();
    m[0][0] = 2/(right - left);
    m[1][1] = 2/(top - bottom);
    m[2][2] = -2/(far - near);
    m[3][3] = 1;
    m[3][0] = -(right + left)/(right - left);
    m[3][1] = - (top + bottom)/(top - bottom);
    m[3][2] = - (far + near)/(far-near);
    
}

/*Vec3f* Matrix::MultiplyVec3f(Vec3f* vec)
{
	float fVec[4] = {vec->GetX(), vec->GetY(), vec->GetZ(), 1.0f};
	float fResult[3] = {0.0f, 0.0f, 0.0f};
	for(int j = 0 ; j < 3 ; j++)
	{
		float r =0;
		for(int i = 0 ; i<4; i++)
		{
			r += m[i][j]*fVec[i];
		}
		fResult[j] = r;
	}

	Vec3f *vResult = new Vec3f(fResult[0], fResult[1], fResult[2]);
	return vResult;
}*/